#include "atos_fifo.h"
#include <string.h>

#if defined(_UCOS)

#if OS_CRITICAL_METHOD == 3
#define ATOS_FIFO_INIT(fifo) OS_CPU_SR cpu_sr = 0  /* Allocate storage for CPU status register */
#else 
#define ATOS_FIFO_INIT(fifo) static uint8_t unused_variable
#endif

#else
static osa_mutex_t atos_fifo_mutex;
#define ATOS_FIFO_INIT(fifo)  static uint8_t unused_variable
#endif

#if defined(_UCOS)
#define ATOS_FIFO_LOCK() OS_ENTER_CRITICAL()
#define ATOS_FIFO_UNLOCK() OS_EXIT_CRITICAL()
#else
#define ATOS_FIFO_LOCK() osa_mutex_lock(atos_fifo_mutex)
#define ATOS_FIFO_UNLOCK() osa_mutex_unlock(atos_fifo_mutex)
#endif

#ifndef ATOS_FIFO_COUNT
#define ATOS_FIFO_COUNT 8
#endif

static atos_fifo_t atos_fifo_list[ATOS_FIFO_COUNT];

void atos_fifo_init()
{
	uint8_t i;
	for (i = 0; i < ATOS_FIFO_COUNT; ++i)
	{
		atos_fifo_list[i].buf = NULL;
	}
#ifndef _UCOS
	osa_mutex_create(NULL, &atos_fifo_mutex);
#endif
}

atos_fifo_t *atos_fifo_new(void *buf, uint16_t capacity)
{
	uint8_t i;
	atos_fifo_t *ret = NULL;
	OSA_ATOMIC_DECL(_lock);
	if (buf == NULL || capacity == 0)
	{
		return NULL;
	}
	OSA_ATOMIC_LOCK(_lock);
	for (i = 0; i < ATOS_FIFO_COUNT; ++i)
	{
		if (atos_fifo_list[i].buf == NULL)
		{
			ret = atos_fifo_list + i;
			ret->buf = (uint8_t*)buf;
			ret->capacity = capacity;
            break;
		}
	}
	OSA_ATOMIC_UNLOCK(_lock);
    
    if (ret != NULL)
    	atos_fifo_clear(ret);
	return ret;
}

void *atos_fifo_delete(atos_fifo_t *fifo)
{
	if (fifo != NULL)
	{
		void* ret = fifo->buf;
		OSA_ATOMIC_DECL(_lock);
		OSA_ATOMIC_LOCK(_lock);
		fifo->buf = NULL;
		OSA_ATOMIC_UNLOCK(_lock);
		return ret;
	}
	return NULL;
}

void atos_fifo_clear(atos_fifo_t *fifo)
{
	ATOS_FIFO_INIT(fifo);
	ATOS_FIFO_LOCK();
	fifo->size = 0;
	fifo->head = 0;
	fifo->tail = 0;
	ATOS_FIFO_UNLOCK();
}

INLINE void atos_fifo_push_impl(atos_fifo_t *fifo, void *data, uint16_t len);

uint16_t atos_fifo_size(atos_fifo_t *fifo)
{
	uint16_t sz;
	ATOS_FIFO_INIT(fifo);

	ATOS_FIFO_LOCK();
	sz = fifo->size;
	ATOS_FIFO_UNLOCK();
	return sz;
}

DLL_API void atos_fifo_prepare_init(atos_fifo_prepare_t *prepare)
{
	prepare->size = 0;
	prepare->succeed = true;
}

DLL_API void atos_fifo_prepare_byte(atos_fifo_t *fifo, atos_fifo_prepare_t *prepare, uint8_t data)
{
	ATOS_FIFO_INIT(fifo);
	if (!prepare->succeed) /* Failed before, then return */
	{
		return;
	}
	if (fifo == NULL)
	{
		prepare->succeed = false;
		return;
	}
	ATOS_FIFO_LOCK();
	/* Overflow, this byte can not be pushed into buffer */
	if (prepare->size + fifo->size == fifo->capacity)
	{
		prepare->succeed = false;
	}
	else
	{
		fifo->buf[fifo->tail++] = data;
		fifo->tail = fifo->tail == fifo->capacity ? 0 : fifo->tail;
		prepare->size++;
	}
	ATOS_FIFO_UNLOCK();
}

DLL_API void atos_fifo_prepare(atos_fifo_t *fifo, atos_fifo_prepare_t *prepare, void *data, uint16_t len)
{
	ATOS_FIFO_INIT(fifo);
	if (!prepare->succeed || len == 0) /* Failed before or empty buffer, then return */
	{
		return;
	}
	if (fifo == NULL)
	{
		prepare->succeed = false;
		return;
	}
	ATOS_FIFO_LOCK();
	/* The fifo is full and didn't have enough space to storage the data buffer with len Byte(s)*/
	if (fifo->capacity - (prepare->size + fifo->size) < len)
	{
		prepare->succeed = false;
	}
	else
	{
		atos_fifo_push_impl(fifo, data, len);
		prepare->size += len;
	}
	ATOS_FIFO_UNLOCK();
}

DLL_API void atos_fifo_push_prepare(atos_fifo_t *fifo, atos_fifo_prepare_t *prepare)
{
	ATOS_FIFO_INIT(fifo);
	ATOS_FIFO_LOCK();
	if (prepare->succeed)
	{
		fifo->size += prepare->size;
	}
	else /* Failed, reset tail with head + size */
	{
		uint16_t remain = fifo->capacity - fifo->head;
		if (remain >= fifo->size)
		{
			fifo->tail = fifo->head + fifo->size;
		}
		else
		{
			fifo->tail = fifo->size - remain;
		}
	}
	ATOS_FIFO_UNLOCK();
}

DLL_API bool atos_fifo_push_byte(atos_fifo_t *fifo, uint8_t data)
{
	ATOS_FIFO_INIT(fifo);
	if (atos_fifo_is_full(fifo))
	{
		return false;
	}

	ATOS_FIFO_LOCK();
	fifo->buf[fifo->tail++] = data;
	fifo->tail = fifo->tail == fifo->capacity ? 0 : fifo->tail;
	fifo->size++;
	ATOS_FIFO_UNLOCK();
	return true;
}

DLL_API bool atos_fifo_push(atos_fifo_t *fifo, void *data, uint16_t len)
{
	ATOS_FIFO_INIT(fifo);
	if (atos_fifo_left(fifo) < len)
	{
		return false;
	}
	if (len == 0)
	{
		return true;
	}
	ATOS_FIFO_LOCK();
	atos_fifo_push_impl(fifo, data, len);
	fifo->size += len;
	ATOS_FIFO_UNLOCK();
	return true;
}

INLINE void atos_fifo_push_impl(atos_fifo_t *fifo, void *data, uint16_t len)
{
	uint16_t end_len;
	uint16_t begin_len;
	uint8_t *tail_ptr;
	end_len = fifo->capacity - fifo->tail;
	tail_ptr = fifo->buf + fifo->tail;
	if (end_len > len)
	{
		memcpy(tail_ptr, data, len);
		fifo->tail += len;
	}
	else
	{
		memcpy(tail_ptr, data, end_len);
		begin_len = len - end_len;
		memcpy(fifo->buf, (uint8_t*)data + end_len, begin_len);
		fifo->tail = begin_len;
	}
}

DLL_API uint16_t atos_fifo_pop(atos_fifo_t *fifo, void *data, uint16_t len)
{
	ATOS_FIFO_INIT(fifo);
	uint16_t sz = atos_fifo_size(fifo);
	uint16_t end_len;
	uint16_t begin_len;
	uint8_t *head_ptr;
	if (sz < len)
	{
		len = sz;
	}
	if (len == 0)
	{
		return 0;
	}

	end_len = fifo->capacity - fifo->head;
	begin_len = len - end_len;
	head_ptr = fifo->buf + fifo->head;

	if (data != NULL)
	{
		if (end_len > len)
		{
			memcpy(data, head_ptr, len);
		}
		else
		{
			memcpy(data, head_ptr, end_len);
			memcpy((uint8_t*)data + end_len, fifo->buf, begin_len);
		}
	}
	ATOS_FIFO_LOCK();
	fifo->size -= len;
	fifo->head = end_len > len ? fifo->head + len : begin_len;
	ATOS_FIFO_UNLOCK();
	return len;
}

DLL_API uint16_t atos_fifo_pop_byte(atos_fifo_t *fifo, uint8_t *data)
{
	ATOS_FIFO_INIT(fifo);
	if (atos_fifo_is_empty(fifo))
	{
		return 0;
	}

	if (data != NULL)
	{
		*data = fifo->buf[fifo->head];
	}

	ATOS_FIFO_LOCK();
	fifo->size--;
	++fifo->head;
	fifo->head = fifo->head == fifo->capacity ? 0 : fifo->head;
	ATOS_FIFO_UNLOCK();
	return 1;
}
